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An error ellipse plotting routine was developed tc 
provide real-time indication of Kaiman filter performance. 
The study included an evaluation of the Hewlett-Packard HP-86 
computer system's capability for providing real-time 
tracking information and an evaluation of the computer's 
possible use on the three-dimensional underwater tracking 
range at the Naval Underwater Weapons Engineering Station, 
Keyport, Washington. A series of tracking runs were used 
to demonstrate both linear and extended Kalman filtering. 
Information obtained from the error ellipses was used to 
modify filter parameters for improved filter performance. 

It was found that the error ellipse was useful as a tool 

for indicating filter performance and for making decisions 
regarding filter parameter modification. The HP-86 provided 
accurate, reliable results and it could be used for on-line 
graphics. However, the computing speed fo the HP-86 computer 
as used in this study was too slow for on-line processing 


of the three-dimensional tracking problem. 
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ifaw NU RODUCT TON 


The Kalman filter's importance as an estimator and 
predictor is well documented. Providing real-time informa- 
tion concerning filter performance so that on-line adjust- 
ments to filter parameters can be made continues to be an 
area of high interest. This study investigates the 
usefulness of the error ellipse as a tool for providing a 
real-time indication of filter performance. 

Part of the investigation involves an evaluation of the 
Hewlett-Packard HP-86 computer system's capacity to operate 
in a real time tracking environment, and its capabilities for 
providing information concerning filter performance. The 
rationale behind this investigation is based on the require- 
ment for the Naval Underwater Weapons Engineering Station, 
Keyport, Washington, to accurately acoustically track 
torpedoes on a three-dimensional underwater tracking range. 
A knowledge of the range operation is not within the scope 
@uemmias Study. It 1s sufficient to Know that presently 
the range receives four time measurements every 1.31 seconds, 
and these measurements are nonlinear functions of the 
torpedo position. 

To gain a better understanding of the error ellipse, a 


4-state tracking scenario was chosen for this study. 





Initially, the linear tracking problem is discussed, 
followed by an investigation of the nonlinear problem. Of 
primary interest are on-line methods to improve filter 
performance using information provided by the error ellipse 


for filter parameter modification. 





eee th Cte Rk) THEORY 


Pee GLNEAR MATHEMATICAL MODEL 
me lie Plant 
For this model the state and measurement equations 
for the plant are linear. Hence the discrete form is used. 
fiewesouned plant model is described by a linear, vector 


difference equation: 


xCkt+t1l) = ox(k) + Auk) + TPwlk) (State Equation) CZ} 


and a linear, vector measurement equation: 


mee) = ex(k) + vk) C72) 
where: 
x(k) is an n-dimensional column vector, denoting the 


Seton oe Ene plant at.“ time’ k. 


uCk) is the deterministic control input, an m-vector, 
at time k. 


w(k) 1s a p-dimensional vector representing any random 
HOw IMD ES at eame k. 


z(k) is a q-dimensional vector representing measure- 
ments made at time k. 


v(k) 1S a q-dimensional vector representing random 
measurement made at time k. 


wel ana C dre assumed Constant Coefficient matrices of 


dimension nxn, nxm, nxp, and qxn respectively. 
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yeeeticoitsce Processes 


DeOmteaeeemr i aee proba biiastic structure on the 


noise processes v(k) and w(k) the following assumptions are 


made: 


fe) ead (he) are andtyidudaliy white processes, 


a Om i ececmelae wlth kz 1, v(k) and 


2 eae Ges ne enaamandon Variables, and w(k) 


Ane Gly mele eENco pendent acdacom yarlables. 


Geeta wand wk) are individually zero mean, 


Gaussian processes with known covariances. 


wep Go eondew lic) are independent processes - 
motemror the measurement noise: 
MEAN: Eee) = 0 C—O ee ee 


COVARIANCE: ELv(k)v"(1)] E{v(k) JELv!(1)] 


= 0 Ko 
= R ean 
= R - 
or Elv(k)v! (i1)j = Roy Cea) 1 2 oni 
where 514 Po weVemicpenoecker delta fumetton defined as: 
=F eae) | 
6 { 
P= i,k = 1 


0 


C23") 


(2-4) 





Likewise for the random forcing input: 


Miie(k)} = 0 (k=9,1,2,3,...) (255) 
T A : 
Etw(k)w (1)J = Q 6,  k,150,1,2,3,...) (255) 
Q and Rv are nonnegative definite symmetric for all 
fm sO since v(k) and w(k) are zero mean and independent 
ieee 
qT 7 
ELv(k)w (1)J = OQ 


(2-7) 


For the purposes of this study, unless otherwise 


specified QQ, and Rv are considered to be known and constant, 


although both may be time varying. 


Meee initial State Description 


For the initial state of the difference equation (2-1) 


ieee unlikely that x will be available. Hence, it is 


assumed that x Memo Gaucetan PanGgecimevamptraple Gp. known mean 


x and known covariance P_ , 1.€ée., 
=O —O 


ix (0) J 


el (x = x 


me 
O 
os 
a 
|x 
' 
ei 
1 
tJ 
c 
UU 


dia 





This choice for the initial state has the advantage 
of causing the subsequent estimation scheme to be unbiased 


aaa ct. Ret. 1] Further it is assumed that the initial 


mmeee ana the measurement noise are uncorrelated: 


aL 


E[x(0)v' (k)] = Elv(k)x'(0)] 0 (k=0,1,253,.6) 
Also the initial state and the random forcing input are 


uncorrelated: 
‘Ie 38 
abl Coy] Elwtk)x (€0)] = © (k=0,1,2,3,...) 


Eee LSCRETE-TEIME ESTIMATION 
im lhe Bee oy a lea eyels 

The estimation problem involves generating an 
amen GStimate for x(j) for the system described by the 
difference equation (2-1) from the noisy measurements 
femme) ,-.-.5,2(3). This estimate will be denoted by 
x(3/4) 5 Teatmmicancmtne CStimate Of x at time 3 given 
measurements at times up to and including time j. The 
estimate must be optimal in the sense that the expected 
value of the sum of the squares of the error in the estimate 


moo Minimum, 1.e.: 


E{L(x(k/k) - x(k) ]°0x(k/k) - x(k)]} = minimum 


leZ 





The estimator is characterized by the linear relationship: 


mA ear ClO), 15 2 sa.) 


Sone 
—— 


x(kK/K) = x¢k/k-1) + 6(k)Cz(k) - 


where 


x(k/k) is the optimal (minimum variance) estimate of 
x(k) given observations at times up to and 
pyar llUlalaliavery Lee 


paces K- 1) is the optimal one-step prediction of x(k) 
given observations at times up to and 
pip gton blots ocean ae 


Bk ) is the optimal estimation gain matrix which 
will minimize the variance of estimation 
error. 


POpeeNvoe iid tic estimare ~C07 0), the estimator 
wemeron (2-8) is initialized with x(0/-1), which is not a 


memcdem variable. If xC0/=-1) fmecloased SUG tha - 


x(O/-1) = E{x(0)] = X, 


Mme be shown that this choice of x(0/-1) makes the 
Paetmeeer Unbiased for all k, [Ref. 1] The estimators best 
available information concerning x(k-1) is the estimate 


x(k-1/k-1), therefore it is reasonable to assume that 


oe 


x(k/k-1) = $x(k-1/k-1) + su(k-1) (2-9) 


is the best prediction. 


ibe 





; 


Pie MunieayeecaUaetons (2-5) and (2-9) are the estimator 


Sem@retons, with x(0/-1) = x, as the initial condition. 
2. Gain and Covariance Equations 


Without going into detailed derivations, the optimal 
Seeeeemacor fains, G(k), used in the estimator equation (2-8), 


are those which satisfy: 


= 


Gk) = P(k/k-1)C°[CP(k/k-1)C + RI] (2-10) 

P(k/k) = [LI - G(k)CIP(k/k-1) (OM) 

P(k+1/k) = oP(k/k) 6" + Q (DaID) 
Pee ne initial conditions: 

P(0/-1) £ Po = E{{x(0) - X I{x(o) - X17} 


where 
P(k/k) = E{Ex(k/k) - x(k) {x(k/k) - x(k) 17} 


[Pemeeme Covariance of estimation error matrix. 
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P(k/k-1) = E{[x(k/k-1) - x(k) ]Cx(k/k-1) - x(k) ]°} 


is the covariance of one-step prediction error matrix. 


JE 


Q = ELR(k)-wlk)ew Cer? (kd 


mMemeene state excitation matrix. 


Bee eend P(K/k-1l) are symmetric, positive definite matrices. 

Several observations can be made concerning the linear 
Kalman gain (2-10), covariance (2-11, 2-12) and estimator 
(2-8) equations. 

Cee uteme jena khomesaens. Gli), do not depend on the 
measurement data and hence can be precomputed,-stored, and 
used as the processing measurements become available. 

(b) Although not obvious from the equation, the 


time-varying gain, G(k), depends in time as: 


Gk) =~ meer. 2 | (2-13) 


This the effect 1s to weight the correction tern, 

Baga Cx(k/k-1)], im the estimator equation (2-8) less 
heavily as time progresses. The advantage of a greater 
initial weight allows for possibly large differences 
Peeween a2(k) and x(k/k-i) during the initial observations, 


and a large gain will result in a significant change in the 


is 





next estimate. This advantage is also borne out in that 
there is less confidence in the quality of the estimates 
during the early observations compared with the quality 

after numerous observations. Hence the later an observation, 
the less drastic an estimate will be altered or affected by 
an isolated observation discrepancy. 

(c) In general, the variance of estimation error 
decreases in a manner analogous to the gain schedule (2-13), 
1.e€., it decreases as k grown larger, reflecting greater 
confidence in the estimate as the number of observations 
Mrereases,. selection of the proper initial condition, Py? 
1s important when studying the effect of measurement errors 
on the behavior of the estimate. So ee should be assigned 
pessimistic values which would correspond to a lack of 
information about the initial state. In cases when the 
initial state is completely unknown, then Poe i. [Ref. 3] 

eee Macrix Serves co compensate ror model 
errors and prevents the covariance matrix from becoming too 
small or optimistic. A small covariance matrix would result 
in a small filter gain, and subsequent observations are 
essentially ignored, which could result in filter divergence. 
Meewoematrix prevents G(k) from approaching zero by adding 
Mm@eemtainty to the system which is reflected in a 


Meeeadation Of Certainty (increase in P(kt1/k)). 
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See NONLANEAR ESTIMATION - EXTENDED KALMAN FILTER 

In many practical applications, the state equations 
and/or measurement equations are nonlinear. Before the 
Kalman filter equations can be used, the problem must be 
linearized and the Kalman filter equations are applied with 
pemen modification. 

1. Nonlinear Model 

Consider a nonlinear discrete system of state and 


observation equations given by: 


m@@cet) = £f(x(k), u(k),k) + w(k) (Pol) ) 


and 


IN 
om 
ee 
ww 
14 


n(x(k), k) + vk) (2-15) 


ieee sewequariOns f£ and hare nonlinear functions 
Smpeeeestacte Variable x, w(k) is the plant excitation A OCee 
“mec 15 the measurement noise. The plant noise and 
measurement noise are assumed to be uncorrelated, zero-mean, 
and white. The same equations (2-3 thru 2-7) apply as for 
the linear model. 

ae esenace) Kalman Filter Equations 

In order to apply the linear filter equations, 
equations (2-14) and (2-15) are expanded about the best 
estimate of the state at that time and only the first-order 


merms are kept. 
ees 





Mie oe, GCetining AC(k) as: 


S30 
Beek) = ae | CoG Ce), kk) 
— “a — — 
and 
ae | eee 
me) = = | (x(k/k-1)) 
— “« — 


As can be seen from the above equations, the filter 
estimates, x(k/k) and x(k/k-1) are used as the "best" 
estimates about which the linearization is performed. The 
fteeeeees A(k) and H(k) must be used to generate G(k) so it 
Mmeeetidble to process Z(k) when it is obtained. The 


modified extended Kalman filter equations are then: 


Gain Equation: 
ae 


G(k) = P(k/k-1)H" (k)CH(K) + PCk/k-1) - HD (k) + RI? 


(2-16) 


Filter Update Equation: 


x(k/k) = x(k/k-1) + G(k)£z(k) - h(x(k/k-1))] Orr 
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Beeatetion Equation: 
x(k+1/k) = £(x(k/k), ulk), k) 

eereerance Of Estimation Error Equations: 
P(k/k-1) = A(k-1)P(k-1/k-1)A) (k-1) + Q(k-1) 


P(k/k) = [I - G(k)H(k) ]P(k/k-1) 


Peete etm tial estimate x(0/0), equation (2-17) is 


mimi 11 Zed with COVE ) lacs) 


x(0/-1) = E[x(0)] = X 
— = =o 


x(0/-1) feces cd ho Initially evaluate H(k). 


Ps an the linear case: 


Is. 


(2-18) 


(2-19) 


(2-20) 





TIT, ERROR ELLIPSOIDS 


Poel oOELORY 

Since the estimate x(k/k) is unbiased in the Kalman 
Memeernequations, the P(k/K) matrix represents the covariance 
of the error in the estimate. If the estimate were biased, 
P(k/k) would represent the second-moment matrix rather than 
Piemeevariance matrix. Hence, P(k/k) provides significant 
information about the accuracy of the estimate. If the 
Dhysical model is accurately described by the state and 
Meeemmemene equations (2-1, 2-2), then P(k/k) can be used 
to describe the manner in which os estimate converges 
(or diverges) to the true state. Examination of the P(k/k) 
matrix directly, element by element, 1s not a realistic 
approach, since the matrix contains ac elements, wnere nis 
m@emmumber Of State variables. To simplify the situation 
the concept of the error ellipsoid is used. {Ref. 4] 

As discussed earlier, the assumptions are made that the 
iitetal state of the plant x is Gaussian, as are the random 
processes v(k) and w(k). Using these assumptions it follows 
Pc ik) and %(k/K) are also Gaussian since they are linear 
combinations of Gaussian variables and deterministic 
quantities. Using the same rationale, the estimation error, 


Gewaned as: 
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e(k/k) & x(k/k) - x(k) 


1s also Gaussian. Using the fact that the mean of the 
Permeation error is 0, the probability density function for 
fige7k) is: 

i 


1/2,-1 


ptetk/k)] = [(27)° “| P(k/k) | 


exp[-1/2e" (k/k)P7*(k/k)e(k/k)] (3-1) 


The density function, p,le(k/k) J Wi lieeiaa ve a seons tant 
value whenever the exponent has a constant value. That is: 


it 


-1/2e° (k/k)P 7+ (k/k)e(k/k) = ¢ 


One 


et (k/k)P 1 (k/k)e(K/k) = 0% (3-2) 


where c iS an arbitrary constant. 

As demonstrated by Sorenson [Ref. 1] and Kirk [Ref. 2], 
mepeam 9 SiOwn that the locus of points e(k/k) which 
satisfy equation (3-2) are hyperellipsoids. For the 
two-dimensional case which is of concern, equation (3-2) 
describes an ellipse. This can be seen by fixing time and 


Bewriting (3-2) as: 
Za 





ous — 


2 
i 11 Mog Myo> equation (3-4) 


describes an ellipse, in which the principal axes do not 


pace w..>0, Wao? O; and w 


coincide with the coordinate axes. The ellipse is rewritten 
mimeeerems Of Y(1l) and y(2) as the coordinate axis, and it can 
Pemoeownm that y(l) and y(2) are the eigenvectors of w with 
Ay and i, defined as the corresponding eigenvalues. [Ref. 2] 
(See Figure 3-1). The equation for the ellipse can be 
rewritten in terms of a coordinate system having unit vectors 


mm~memie directions of Gab) eevnrc! y(2) aomune pasts vectors. 


The ellipse equation becomes: 


2 Z 
Avy. dee Ao¥ G2) t= Ee Ce= 5) 


Dee. 





Pegs 3—il emer lawl pSe 


Pemembpering that w = STE (ee) - Pi redwmee Siown thae the 


corresponding eigenvectors and eigenvalues for Ww =P (K/k) 


emt), y(2), Oy > and Oe 3 where 4 = 


Equation (3-5) can be rewritten: 


ve) y*(2) ; 








eal W. 


Vie 


— and a 
AL 


Z 


Ce le) 





For the purposes of this study, the term error ellipsoid 


refers to the specific SGase Whemee = 1. §S0 equation (3-5) 
becomes: 
y?(1) y (2) 
ae + a = a 
“ai we 


Im terms of the Cartesian coordinates x', y', which use ey 


and e, BMemmasis Vectors: 


Peee— xCOS@ + ysind 


a -xsin@ + ycosé 

where @ is the angle of rotation of the axes and can be 

computed from: 

2cov(e_e ) 
x Y 


] [Ref. 5] 
Weigel Ss sua.) 
a y 


For a given value of ¢c it 1s possible to integrate the 
probability density over the surface of the error ellipse to 
obtain the probability that a particular sample point will 
iMiemirenhin the Gcliipsoid. For this study, n= 2, c =l, 
and the probability the error is inside the ellipse is 0.394. 

In summary, the error ellipsoid can be used to 


characterize the concentration of the estimate about the 
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femmes Value Of the state. A decrease in the magnitude of an 
axis of the ellipse is an indication that the error in the 
Seeeenate 1S decreasing in that direction. 

One important item that needs to be pointed out is that 
often the components of the state vector represent entirely 
different types of variables, for example the components 
might represent range, velocity, and depth. Since the two- 
dimensional ellipses are determined by using two components 
of the state vector, it 1s reasonable to examine submatrices 
relating state variables of the same character. Doing so 
will preclude most scaling difficulties when plotting the 
ellipses, and provide more meaningful insight in to the 


jm@esutcs. 


Seen nOk ELLIPSOIDS AND FILTER DIVERGENCE 

Thus far the discussion has centered around using 
Smeeacrteces OF the P(k/k) covariance of estimation error 
matrix as the input for the error ellipse to indicate 
filter performance. As proposed by Heffes [Ref. 6] and 
Severed | Rer. 7), P(k/k) can be considered as a "design" 
covariance matrix e, uSing the assumption that the only 
errors are in Qi» Rus and ie with the following inequalities 
molding for all k: 


d a 
eee 6 ob (3-7) 


ZS 





with the subscript "d" indicating designed and "a" indicating 
actual. Equation Cap implies more input noise, more 
measurement noise, and more initial state uncertainty in the 
design than actually exists. This conservative filter design 
results in a somewhat pessimistic design error covariance 

Po (k/k). The actual error covariance P*(k/k) resulting 


d 


from using a filter designed with oes ase ond pe 1s related 


to the design error covariance in the following manner: 


P°(k/k) > P°(k/k) 


This result is particularly useful when one simply does 
not know accurately the noise covariance Oh the dimple. or 
output, but an upper bound is known. Designing assuming 
the noise covariance is at its upper bound will result in 
P°(k/k) being upper bounded by Po (k/k) . In some sense a 
worst case design results. Filter divergence exists when 
the design error covariance P“(k/k) remains bounded while 
the error performance matrix P*(k/k) becomes very large 


relative to Po (k/k) Or 1S, in fact, unbounded. 
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Pee ROBLEM DESIGN 

The purpose of the tracking problem is to study the use 
of error ellipsoids as real-time indicators of filter 
performance. In order to keep the design model realistic 
albeit reasonably simplified for ease of study, a 
two-dimensional tracking problem using several different 
“meres Nas been selected. 

Mme hinear Tracking 

All tracks are based on an x-y coordinate system 

with the target moving in the x or y direction relative to 
the sensor located at the origin. Thus for aircraft 
tracking, altitude is considered constant, as is depth for 
Perpedo tracking. 


Defining the state variables as: 


x, = X x-coordinate of the target location. 
xy = x Jeloeilty of = target Og in x-direction. 
a OY y-coordinate of the target location. 
iy amd velocity of target ee iia —-durectLon. 


Za 





resulting in a state vector: 


= (4-1) 


The following are the state equations: 


x(t) = 


x(t) - wt) 
(4-2) 


x(t) = x,t) 


x, Ct) w(t) 

where w(t) and w(t) are assumed to be uncorrelated, 

random processes that account for unknown target accelerations 
and nonlinear target motions. Writing the discrete form of 


the state equations gives: 


Vas) 





x, (kt) 


i$ 
x 
}--/ 

2 
-+ 
Sat 
x 
2 
+ 
= 
ae 


x, (kt1) = x, CK) eee yee) 


il 
Cia) 
ey, 
met) = x,(k) + T * x, (k) + ao Wy (K) 
x, (k+l) = x, Ck) eal w(K) 
OD 
x(kt+1) = ox(€k) + Iwlk) (4-4) 


With T, the sampling period equal to 1 second, the matrices 


d and ieare: 


ie ec: 0 5 (0 
Como CO 1h - 
> = [bee Ges) 
Como 2. Ol Capen 
tre Oo 1 Cpe 


It 1s assumed that the sensor gives noisy, but 
uncorrelated measurements of x and y. Hence the discrete 


measurement equations are: 





Zz, (0k) x Ck) : V4 ¢k) 


(4~6) 


ZK) = x4 CK) a V7 (k) 


Wee i v6) 2G V7 Ck) imeem ha roqleranGem 101Se. 


Thus for the measurement equation: 
mea = ex(k) + v(k) (4-7) 


mPemmackrilx Cc is: 


For the initial run and unless otherwise noted the 
following values for the Gaussian random processes will be 


used: 


Bev (Ck) J =e oma] i 
= DScLO” 0 a 


Elv(k)v" (k) I = m = R for all k 


3) 





O = | ieee orangdarda deviation of 


gos measurement noise. 





TACK. 0 
al - Le 
Elwlk)w (k)] = Gisee te = cov w for all k 
_0 71,010. 
~ 10 
- inee = Pier sceong@ard deviations of the 
tom Baten ere ling. dimple . 


leeetovariance of estimation error matrix is initialized: 


10 0 0 0 
0 10% 9 0 
eeeP(o/-1) = ; 
i 0 0 10 0 | 
0 0 0 igs 


om 





gece filter as to be unbiased, the initialization: 


x(0/-1) = x = Initial condition of the problem. 
@y Nonlinear Tracking 


The state equations are the same as for the linear 
tracking problem. The measurement equation is considered 


as a noisy range measurement by the tracking sensor and is 





@m@aracterized as: 


AG = (x4 (i) : xo(K) 14"? + vy Ok) (4-8) 


Thus z(k) 1s a nonlinear function of the states. Using 


equation (2-14) and with T = 1 second: 


— &, CK) + Ky (kK) 
x CK) 
f(x(k), u(k), k) = 
_ x(k) + x, CK) 


i. &,, 0K) 


8) 





Taking partial derivatives of f with respect to x gives: 


Q? 
rh 
ae) 
eat 
(SB) 
& 


A(k)= 


ran 


I (x(k/k), uk), k) a 0) Mere 





Pememeeeguation (2-15): 


n(x(k), k) = [xo(k) + x2(x)]1/? 


wemeeicing the Dartial derivative with respect to x gives: 


dh j x, CK) 5 We 


i aS eae 
x4 (k)#x5(k) J : 


H(k) 


a? 
[x 


GaGic/ ici) ) 


| 
x. CK) ; 0 | 
Oe Sl aera es ‘i 
[xi (k)#x5(k) I : . 

pokey ic) 


313) 





Using the above resuits with the same values for the 
random noise processes as previously stated for the linear 
case, the extended Kalman filter equations can now be applied 


to the nonlinear tracking problem. 


PemeeocoMeUTER SIMULATION 
1. Computer Hardware/Software 
a. Hardware 

All computer sinmiLacions were run on the Hewlett- 
Packard HP-86 personal computer. This particular model was 
chosen to evaluate its capabilities in determining its 
usefulness in actual torpedo tracking at the underwater 
tracking range at Naval Underwater Weapons Engineering 
Station, Keyport, Washington. The HP-86 system used included 
keyboard, 9 inch CRT monitor connected through an integrated 
monitor interface, and two HP Flexible Disc Drives connected 
through an integrated disc interface. Plotting was done on 
a HP-7225B Plotter and printing on a HP-2631G Printer. These 
peripherals were interfaced using a HP-IB Interface module. 
Because the system uses interface select codes, the HP-I38 
Factory preset code was set at 7, which is the select code 
For the printer/dise interface. This code however did not 
work when interfacing with the external plotter and printer, 
Since duplicate select codes are not allowed. So the 
internally set select code of the HP-IB was set to 8 for 


proper system operation. 
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Pe  oOfFtWware 

The HP-86 has 60K built-in, useable bytes of 
computer memory, expandable to 572K using either 32K, So4K, 
or 128K Memory Modules. A HP-86 plug-in ROM was required 
to operate the external plotter. Also a Matrix ROM was used 
Mem@eauce program length and computer run time. 

All programs were written in BASIC programming 
ieroteee Using REAL (full) precision, which provides 15 digit 
precision. Appendix B provides an explanation of the program 
options and Appendix C contains the program listings used for 
this study. 

2. Track Generation 
To evaluate the real-time use of the error ellipse as 
an indicator of filter performance, Monte Carlo simulation 
Gemyecre made. Four tracks were generated by separate 
programs and one second incremental values of x, y, oe and 
Ue were stored in data files. Appendix I contains an 
explanation of the generation of tracks three and four. 
3. Noise Generation 
In order to simulate the random noise processes, the 
computer's random number generator was used and the generated 
numbers scaled accordingly. For each track and each 
different value of noise sigma, a different generator "seed 
number" was used. These noise values produced were added to 
the applicable true track values to simulate a sensor 


measurement corrupted by independent Gaussian noise. For all 
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cases where filter parameters were varied for a particular 
track under a specific noise condition, one noisy track was 
Senerated, stored, and used throughout that particular 
Simulation. This was done for ease of filter performance 
comparison. 
4, Gating Scheme 

iimorder tO preclude GCatastropnic filter failure due 
to excessive measurement noise, a bound was established for 
the maximum acceptable limits of measurement noise. A 
three-sigma gate was designed using the covariance of the 
feeem@emenit noise, R, and the predicted covariance of error 
ieee (K/K-1). The gate is defined as: 


Gate(k) = 3(p,,  (k/k-1) + R,,)1/?, 


Aas 
max 


This gate 1S the maximum error allowable for the measurement 
at time k. If the absolute difference between the actual 
measurement received and the predicted measurement is 
greater than the three-sigma gate, then that particular 
measurement data is rejected as unacceptable. When this 
Seemeome che filter gain, G(k), is set to zero, resulting in 
that measurement being ignored and the prediction of the 


States set equal to the estimate, that is: 


BG =) x (c/ K+ 11) 


Sis 








Pe collection of Statistics 
In order to study error ellipses as an indicatcr of 
mmheer performance, statistics were calculated, on line, 
after each measurement during the Monte Carlo run. The 
statistics computed were relative error (in some cases the 
error was normalized), error mean, error variance, and error 
covariance for the positional variables. The following 


equations apply: 


Relative Error = e(k/k) = x(k) = x(k/k) 


Geter error residual) 


k 
Error Mean = e(k/k) - 1/k & e(3j/}) 
jel 
s = 2 
Meer Variance = Varle, (k/k) J Pee ka Oy roeiG ale] Geni) | 
ime 
L=Ky 5 X50 Xq0 Xy 
K 
Varle, (k/k) ] IV le, (j/j)ile, Giaiel 
i oat 3 
fect ecional 
Error : ee anes Cc/ic) | 
Covariance a 3 
Richter x 
k 
Il Brom) Fie ya) ker Gy ae Varle. (k/k)] 
jones <1 3 3 


-Le (k/k)°e (k/k)] 
= Si 


od 





fee koed TRACKING AND ERROR ELLIPSE ANALYSIS 


Pee GLNEAR TRACKING 


Track 1 depicts a target approaching at a constant 


velocity of 223.6 feet ver second. The solid line 
5.1 indicates the true track of the target and the 
along the track indicate the time in seconds. The 


was tracked in a measurement noise, Sa SO Ww 1 ta 


random forcing noise Oa ees scumrone20,000. 


of Figure 
numbers 
re Men ee 
che 


The 


dots indicate the filtered track using the linear Kalman 


filter equations. Figures 5.2 and 5.3 are the filter error 


ellipses for this run, computed at increments of 10 seconds. 


As can be seen the ellipse size decreases with increasing 


time indicating filter convergence. The computed ellipse 


surface areas shown on the figures confirm this. 


Smee nune tiltered track for the case where oe has 


Figure 5.4 


been 


increased to 300 and all other parameters remain the same. 


The error ellipses of Figure 5.5 computed for 10-second 


increments show increasing area indicating filter divergence. 


Figure 5.6 shows the error ellipses for the same track run 


but this time the ellipses were computed using a "statistics 


window" of 10. By this is meant that the ellipses were 


derived from statistics computed for the last 10 data 


measurements. All previous data is disregarded. Using this 


38 





Meeegede tne eCllipses Of Figure 5.6 show filter convergence 
Mmeemereterations 15 te 25. The filtered track of Figure 
Sereecontirms tnis. figure 5.7 snows the results for the 
same track parameters, except in this aie. 2 statistics 
window of 5 was used. The window 5 ellipse area at time 
meee os sq ft) is much less than the area of either the 
Bemee tle Statistic window of 10 (11,540 sq ft) or the 
run with no window at all (65,500 sq ft). This is expected 
Since the filter is essentially "locked on" at time 20, 
and the window 5 ellipse at time 25 disregarde all data 
previous to time 20. Figure 5.8 shows the error ellipses 
for the same track but the measurement noise was increased 
to = S005 while KR was Kept at 20,000. The error ellipses 
indicate filter divergence, and indeed the filtered track 
headed off in the wrong direction. 

Track 2 depicts a target approaching at a constant speed 
of 500 feet per second in the -y direction. Figure 5.9 
depicts the solid line track and the dots Mein te the linear 
filtered track with ae Setsrte es mowed = 20geee. Faure 
Seeeeend 5.11 are the error ellipses for the run. No 
statistics window was used. Other than the fact that the 
ellipse area is decreasing, the shape of the ellipse provides 
ieee additional information. Figure 5.12 and 5.13 show the 
ellipses for the normalized error. With the same measurement 
noise sigma for both the x-position and y-position measurements, 


the normalized error ellipse's shape and orientation reflect 





the target track proximity to either axis. In Figure 5.12 
the eliipse major a indicates a large normalized error in 
the x-direction. This is to be expected since the target 
maintains a constant x-position of 500 feet, while the 
y-position is initially 20,000 feet. However, near time 40 
as the target approaches the x-axis, the normalized y error 
increases and becomes so large at the x-axis crossing that 
the normalized x error becomes insignificant in comparison. 
This can be seen in Figure 5.13 for the ellipses at time 

45 and 55 compared with the ellipse at time 35. The 
normalized error ellipse is an excellent indicator of 

Peer proximity to an axis, but the rapid shifts in ellipse 
surface area make it difficult to determine filter 
convergence. 

Track 3 depicts a target approaching on a parabolic 
track at a speed of 200 feet per second. Figure 5.14 is the 
true track, with a linearly filtered track indicated by the 
fem) FOr this run o,=150, o=10, and R=20,000. Figure 5.15 
are the error ellipse plots for times 40, 50, and 60, the 
period of the highest rate of change in x- and y-velocity. 
The ellipse areas increase with time indicating divergence. 
Figure 5.16 and 5.17 are the ellipse plots using a 10-data 
point and a 5-data point statistics window respectively. 

In both cases the ellipse areas for time 60 are less than 


time 50 indicating the filter has tracked around the curve. 
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Using error ellipses based on a statistics window in this 
tracking situation provides a better indicator of filter 
periormance. 

bomethe second run of track 3, o,, was increased to 300 
and all other parameters remained the same. Figure 5.18 
Miemiemene resulting filtered track, which obviousiy didn't 
track around the curve. With the large amount of measurement 
noise and considering that the maximum random forcing input, 
1.e€. the maximum acceleration in the x~- and y-direction, 
occurs between times 40 and 60, it is a logical place to lose 
track. Another factor to be considered is the decrease in 
gain as k increases. By time 40 the gains have little 
influence. So a system was incorporated in the program to 
Deemmectalaze' the filter by setting the gains to G(0), if 
Gertain Conditions were met. After several trial and error 
runs, it was determined that if a statistics window of 10 
were used, and the gains reinitialized if the error ellipse 
area increased consecutively a certain number of times, 
that the filtered track would follow around the curve. 
Figure 5.19 is the filtered track using a statistics window 
of 10 and reinitializing the filter if the error ellipse area 
increased consecutively during five l-second increments. 
Mtewerror ellipses for that run are shown in pmeciees 0.20 
meee. 2). AS indicated on the figures, the filter was 
reinitialized four times and the ellipse areas for the 


10-second increments increased, until réinitializing the 
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filter at time 52 locked the filter in. Consequently the 
error ellipse areas for time 60 and 70 decreased, indicating 
convergence. 

In the final run, the filter was reinitialized after 10 
consecutive l-second error ellipse increases, with all other 
parameters remaining the same. frigure 5.22 is the filtered 
fede, Figures 5.23 and 5.24 are the error ellipse plots 
meets Yun. As indicated on Figure 5.24 the filter was 
Meeeetialized only once at time 57. A comparison of ellipse 
areas for the last two runs shows that, with the exception 
of time 70, the areas were larger in the first run when the 
filter was reinitialized 4 times. This 1s expected since 
reinitializing results in larger gains producing more widely 
vary estimates, and hence greater error variance. At time 
70 the error ellipse area of the first run is less since in 
the first run the last filter reinitialization occurred at 
a he 52 versus time 57 in the second run. The first 
filtered track had more time to settle out by time 70, 


resulting in less error. 


B. NONLINEAR TRACKING 

Track 4 depicts a target moving at a constant velocity 
of 50 feet per second (30 kts) in the x-direction for 15 
seconds, at which time the target turns and travels in the 
-y-direction. (See Figure 5.25) Using the extended Kalman 


filter with 0,,=30 Mia scmloomem Eedeking runs were 
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made for various values of COVW (o=from Atom 0) ein none 
eeeese Puns did the filter successfully track the target 
meemm@er tne turn, Figtre 5.26 shows the results for the case 
DeemeeeOVW=150. in this instance the filter lost track as the 
Peet Came Out Of the turn. Trying to track through a turn 
Bemege ea Constant COVW (and hence, a constant Q) did not work. 
SO a scheme was devised to vary COVW dependent on information 
derived from the error ellipse. After several trial runs for 
this particular track, it was determined that if the error 
ellipse area increased consecutively for 7 iterations of 

k, COVW would be doubled, and if the ellipse area decreased 
consecutively for 5 iterations of k, COVW would be halved. 
Initially the the trial runs were made without a statistics 
Peeper. and the filter did not track successfully. Without 
the statistics window, the old data weighted down the statis- 
tics, and the error ellipses were not indicative of what was 
currently happening. So it was decided to use a statistics 
window. Windows of 5, 10, and 15 were tried. Window 5 
proved to be too responsive and window 15 not responsive 
enough. So a statistics window of 10 was chosen for the 
moeerine run. With Po=lo°, a. =30, R=900, and COVW initially 
set at 20, the tracking run was made. Figure 5.2/7 depicts 
the filtered track output, and Figures 5.28-5.30 are the 
10-second incremental error ellipses for the run. As 

Can be seen, the filter did track around the curve. Figure 
5.29 shows the ellipse areas are becoming less between times 
95 and 65. Also indicated below the plots are the values of 
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COVW for the k time the plot was computed. During this 
Pe@etcular run COVW varied from an initial value of 20 up 
to 160, and then decreased to 40 by the end of the run. 

Using the same filter parameters as above except Oo. was 
eem@emoed tO 200, and R to 40,000, another run was made. The 
filter did not track at all. During this run COVW varied 
from an initial value of 20 up to 40 and decreased to 5 by 


the end of the run. Obviously, the criteria for increasing 





and decreasing COVW was not effective. More trial runs were 
necessary to determine the optimum consecutive increases 

or decreases of the ellipse areas before adjusting COVW 
a@eerdingly. 

A second approach to the nonlinear tracking problem, one 
that was used earlier for the linear case, is to reinitialize 
the filter if certain conditions are met. Again, using 
eaemieand error runs with and without statistics windows, 
it was determined that using a statistics window of 10 gave 
the best results. Using initial conditions of COVW=150, 
P10. os Ge Oe enamels ewicire Made, reinitializZ— 
ing the filter if the error ellipse area increased consecu- 
tively for a certain number of iterations of k. Of the runs 
attempted, the best results were obtained when the filter was 
reinitialized if the area increased for 5 consecutive 
Mee@aetons Of k. Figure 5.31 is the filtered track for this 


run, and Figures5.32-5.34 are the error ellipse plots. 


Indicated below the plots are the times, k, when the filter 
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was reinitialized. For this particular run the filter was 


MmeEppeeeali7zeda 5 times. It should be noted that when 
ee hin, Oo Ra ae 
Meme eliazing the filter, P(k/k-1) was reset to 106° x P 


Py was not large enough to be effective in getting the 
Filter back on track. The initial value of 10 was used for 
Po to reflect tne high confidence in the initial state 
conditions. Other values of - did not work as well. 

Using the same parameters as above except the filter was 
reinitialized after 7 consecutive error ellipse area 
increases, another run was made with the resulting track 
@emeered in Figure 5.35. A comparison with Figure 5.31 
reveals that using 7 area increases as the criterion for 
filter reinitialization resulted in poorer filter performance, 
as can be shown from the error ellipses. 

The final nonlinear filter tracking runs attempted 
involved simultaneously varying COVW and filter reinitializa- 
tion. The results were disastrous, and highly unpredictable. 
It was an interesting experiment in futility, and no 


meaningful results could be obtained. 
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Vie oleouus LONS mala RECOMMENDATIONS 


Peek OR ELLIPSE 

The filter error ellipse proved useful as a tool for 
indicating filter performance. The information provided by 
the ellipse, particularly surface area changes, was used to 
make decisions concerning the alteration of the filter 
parameters. Several approaches for using the error ellipse 
were applied to both the linear and nonlinear tracking 


problem and the results are summarized below: 


Procedure Applicable Comments 
melcer 
Cima ore 
Extended) 
Peers stics Both Useful in keeping the error 
Window ellipse current and responsive 


to present data. The ellipse 
reflects most recent data; old 
data is disregarded. Beneficial 
when making a decision concerning 
filter parameter modification. 
Normally a better indicator of 
Filter convergence or divergence 
than without a statistics window. 


Normalized Powel Aid in displaying error trends 
Beroer as target approaches coordinate 
Ellipse 2 loeOGvoraeiiweNot preactical 


in determining filter conver- 
gence/divergence due to rapid 
ellipse changes in vicinity of 
axes. 








Reinitialize Linear- 
Filter- Set G(0) 
Increasing Extended-Set 
Ellipse Area P(k+1/k)= 

{ecg anc 

—~O 

mae Or Extended 
Ellipse 
Expansion/ 
Compression 


to vary COVW 
(Adaptive Q) 


Peeler w TER PERFORMANCE 


Use increasing ellipse size as an 
indicator of divergence, and a 
decision-making device to 

Poe etakZze ari lter, Particularly 
valuable later in track when gains 
and P(kt1i/K) have settled out. 
More effective when used in 
conjunction with statistics window. 


This technique of varying COVW 
based on error ellipse area in- 
crease or decrease 1S particularly 
useful in a tracking environment 
containing large variations in the 
BAVC OoneCini wipe. ne proce= 
dure is more effective when used 
im GOMquUnetion with a statistics 
window. Using this technique 

with filter reinitialization was 
lbmisueciess tual. 


The HP-86 proved to be an extremely reliable computer 


with no downtime experienced during the 5 month period of 


Operation. 


Full(Real) precision was used throughout the 


Study providing 15 digit precision, which was more than 


adequate. 


The use of the Matrix ROM reduced program length 


by 90% and increased computing speed by a factor of about 6. 


Although not used in this study, a Statistics ROM would have 


undoubtedly further increased computing speed. 
further study using the HP-86, 


ewes cics ROM be procured. 


For any 


1t 1S recommended that a 


It took approximately 2 seconds for each incremental 


time measurement data to be sequenced through the filter 


equations, both for the linear and nonlinear case. 


luiguae 


BZ 





-y 


Semputing time also included all statistics computations. 
With an incoming data rate of 1 set of measurement data per 
second, this computing speed is not sufficient for on-line 
processing. As previously mentioned, the Naval Underwater 
Tracking Range receives a series of 4 measurement times 
sequentially every 1.31 seconds. The range's three- 
dimensional tracking problem will necessarily involve more 
than the 4 state variables used in this study. Hence 
greater matrix dimensions resulting in longer computing 
times can be expected. 

The HP-86 CRT graphics were used extensively to provide 
pem@emeellipse plots during the tracking runs. Using a "no 
ames approach to plotting, i.e. plotting without x-y 
axis or labelling, it took approximately 2.5 seconds per 
ellipse plot. The ellipse plotting routine used involved 
Sines and cosines, plotted point by point in 30 degree 
increments for a total of 360 degrees. This method was 
somewhat slow. Had there been available a graphics program 
that would sketch in the ellipse around the intersected 
major and minor axis, the graphics presentation could have 
been speeded up. But since ellipse plotting for every 3 to 
5 increments of time provided sufficient "real-time" 
information, the time of 2.5 seconds per plot was tolerable. 

Summarizing, the HP-86 could be used to compute 
statistics and provide graphics in the real-time underwater 


tracking environment, if the graphics were required not more 
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often than 3 to 5 seconds. However, before the HP-86 can be 
considered feasible for real-time Kalman filter processing, 
more investigation is needed in finding ways to speed up 
computer ain C295 LE time such as parallel processing, 
additional use of manufacturer-provided ROMs and machine 


language programming. 
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TRACK GENERATION 


Meek ACK THREE 


Target movement is in the x-y plane with the tracking 
sensor located at the origin of the cartesian axes. The 
meeeet tTOllows a parabolic track (see Figure A-1) ata 


constant speed of 200 feet per second. 
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Figure A-1 Track 3 





Mr] parabolic equation is: 


y= 4p(x-h) 


fete = 1000 and h = 1000, resulting in: 


Wa = 4000(x-1000) 


Meee! target location is (x,y) = (8000, 5291.5). 


x-direction velocity is given by: 
m= cos(Angle) 

and target y-direction velocity is: 
i = Vv sin(Angle) 

where the Angle is obtained fron: 
Paz le = tan ~ (42) 


and v is obtained from: 


2 yey 2 
a, 
Sf 
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(A-1) 


(A-2) 





where the argument of the inverse tangent is the slope of a 
small increment (less than 1 second) at each successive data 


point. Using a sampling period of one second, the data 


points (x,y) can be obtained from: 


T 
x 
oa 
+ 


Packt) ) ECS) 


mkt) VIGO gs Ta 


Table A-l gives the numerical values for the four states. 


ZeeeesACK FOUR 

Target movement simulates a 30-knot torpedo (velocity 50 
feet per second) at a constant depth. The target's initial 
Peeeweron 15 (x,y) = (3250, 5000), and it is moving in the vo 
direction with gue (See Figure A-2.) The target remains 
on a straight course for 15 seconds, and then executes a 90 
degree turn and travels in the -y direction at ee oO ft/sec. 
The trajectory of the target turn is described by a 90 degree 
are of a circle of radius, R = 1000, with the circle centered 
at (x,y) = (4000, 4000). The 90 deg arc will be traversed 


ere 


Smee eeeee = On Seewcwhere Vo=nou ft/sec) 


8 7/ 





aN 


womdwAHinN & WA) = 


X 


€000.CC 
Wis Ne eae 
TEVA 
(Aes) Says 
TAS. SRS 
7067.44 
Scone 2 
S Guo. cz 
Cea. 14 
6212974) 9.0 
E144,14 
SGU Cel 
Soe. Te 
Sogo IC 
S414. 4 4 
3233.49 
S05) S| Saraes 
4873.74 
Mego 2 
GS.) 1S 
4340.19 
4164.22 
S565) See 
Soe 5 1 
ges). OS 
2472.00 
2202.43 
3134.54 
Zee. 2G 
2804.52 
ZOWZ 6 € > 
2483.76 
Ce So 
2174.74 
PAWS) & als 
1880.95 
1741.28 
1607.50 
1480.67 
G70 
lz5ai.3 
USS Gishs 
1074.64 
101171. 20 
1000.C0 


TABLE A-1 


A-VEL 


me Wicks e)0, 
- 186.93 
- 186.60 
= sin Be 
- 185.89 
SWS 2 
se coe 
- 184.68 
-184.24 
ee lkeloics Ute 
we Seo as 
ssi 5 1: 
-182.17 
-181.5/7 
Se LW IShOl Se) 72 
- 180.24 
= el 3). 
isha UZ 
-177.87 
= Io SNe 
= eee 
= ean 
“173,74 
- 172.48 
-171.09 
= 169e0om 
= joy mo 
- 166.04 
me (ests S's 
= loader, 
= Aen 
S156 1 7 
ee a)6 6 18's 
- 149.07 
-144.72 
= 2307 
Sasso its 
me Asi 8S 
-118. 59 
-108. 70 
Sele 5 18 
= 5 ZanOu 
-63 44 
= 37) eye 4 
0.00 


88 


Y 


S294 50 
5220. 39 
5148. 27 
5075.4 26 
SOW 1.33 
4926.43 
4850.54 
4773.60 
4695.59 
4676.45 
4530.14 
GQ4U54.60 
4371.79 
4287.65 
4202.10 
4115.09 
4026.54 
BS) Se 6 
3844.49 
217s ce ey 
3655. 24 
5) 5 i/q es, 
ile 9S 
Bis) spo ences. 
Bi Ae 2 
37144,52 
3034.75 
2526 1 
2806. 06 
2686. 65 
2563.47 
2436. 20 
2304.42 
ZNGis/ 0 
PA)0) 728) sy 
1877. 18 
i 2 oS 
So ae 
WIG ..o | 
1203. 46 
VOOG war 3 
oilice= 6 

5946.41 

AN Ae (2) &: 

0.00 


NUMpBRTCAD VALUES OF STATES FOR TRACK THREE 


ey lu 


a7. 15 
=n st 3 
-71.98 
-72.87 
-73.79 
-74.74 
-75.73 
-76.76 
-77.33 
-78.94 
-80.09 
-81.30 
-82.56 
- 83.87 
-85.24 
- 86.68 
-88.19 
-89.78 
-91.44 
-93.19 
-95.94 
-97.00 
-99.06 
-101.25 
-103.57 
-106.05 
-108.68 
-111.50 
=A eon 
-117.74 
-121.21 
-124.94 
-128.98 
- 133.33 
-138.05 
-143.15 
-148.67 
-154.64 
-161.05 
-167.88 
-175.05 
-182.41 
-189.67 
-196.50 
-200.00 





X 


TO ieeZ0 
G2V 577 
1046.90 
1082.72 
es Wea oo 
WSS 6 
19 7 Be Se, 
e5ou. 31 
TaD 256 
leaec ale 
eyo. 35 
WSIS Ai 
1527..94 
zCo4.01 
2205.24 
Cane ©) Z 
ZeOrarc O 
Z2€54.11 
Zeleo c 
23094 3G 
emo) \e) DZ 
B54 4S 
3461.47 
Geo 2 
Bo Gre Ve 
Bod 4.6 S 
4142.03 
Ue OT Seaton 
4490.38 
4€66.21 
4843.02 
2020. /4 
Salen). 3 z 
Sao. © 
BaD o. | & 
Sieg. s 
ioe.) 0 | 
en W5)60 = 
G25. © 6 
E468. 80 
BoDz.4> 
Seo. 26 
7021. 16 
7206.16 
Poe 7 


Tap ieeA =e CCONT .) 


A-VEL 


10.57 
25.174 
SO 1s 
Ms) 5 ans, 
Suen 
74.49 
36.36 
Sl 5 aS 
107.04 
Wile 7s 
123.43 
EOE) ie 
136.06 
41.24 
145.78 
Wes) 5 Bis 
WSsig S| 
156.45 
159.24 
Grice 5 
legen 
earn Ss 
167.80 
169.46 
Olas She 
i Zee 
173.60 
174.76 
iSiene 2 
TKS) SiS) 1 
WLS 7 
VUES ES 7 
We sis 
180.10 
le Omar 
1871.44 
182.04 
WeZ5 © | 
183.174 
WSS 5 oS 
184.13 
184.58 
isis 6 0 
so) al 
Ley 6 IFS 


che 


mas 63 
AG a) Oh) 
= 35613 
moO 2S 
me cla 7 
is) Seley 
Sore ace © 
=> i chO sats 
= 13945596 
-1498. 80 
= [eB Site 
7A OG. od 
mo ZO etcy 
a2 003.0 | 
ree enol, 
Soe wero 
-2450. 14 
=25 12622 
-2691. 14 
a SNS 6 SIE, 
SESSA SS 
SOL SUO E17 
ee oe) 
=3293.01 
= 33454 26 
-3446, 56 
235400. 15 
“3641.77 
=O. 2 | 
eeo7g. 4 / 
Bore, 12 
mo Oe 3.0 
-4098.45 
SOD 6 0 © 
ae Ole ZO 
-4354.11 
~44U36.67 
“4517.99 
-4598. 11 
LMS) Ft 0s 
-4754.S8 
ae) Stele, 
-4907.61 
“4982. 43 
=o 56. 31 








Figure A-2 Track 4 


590 each second: 


=o radians wall be Enaversed 





Using the trigonometric identity: 


Mpc cos-(A) = 1 
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And the equation for a circle: 


ieeollows that the arc of Figure A-2 is described by: 


hae) = 4000 + 1000sin(.05k) 


mee) = 4000 + 1000cos(.05k) 
where the angle argument is in radians and k=0,1,2,...31 
seconds. The velocities ve and wae can be obtained from: 


Vv (k) SP oe@ce, 0 5x) 
YS sea Ocin (05k) 


uSing the same angle argument. 
Wigtemeum@ack Values for the track arc are contained in Table 


A-2. 
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COMB tei: hOeGhAM Erin Ad LON 


1. LINKAL 

The LINKAL program computes the filter gains, GAIN(4,2), 
for the 4-state system and stores the gains in "LINGAIN 
»-STORAG". The theoretical covariance of error matrix, PKK(4,4), 
is also computed and stored in "LINCOV.STORAG". Several 
different sets of gain and covariance values were computed 
and stored for various values of measurement noise, RMAT(2,2), 


preame@amdoem forcing noise, COVW(2,2). 


fae NEST 
The LINEST program retrieves the appropriate gain 
schedule from storage and computes the optimal estimate, 
Pees), and the optimal one-step prediction, XHKIK(4,1). 
The following capabilities are contained in the LINEST program: 
a. Gating Scheme 
If the absolute difference (DIFF) between the one-step 
prediction, XHK1K and the noisy track value, ZMAT, 1s greater 
than the three-sigma gate, then the GAIN matrix is disregarded 
and XHAT is set equal to XHKLK. 
b. Track Noise 
By setting NOITRAK = 1, the random number generator, 


RND, and the resulting simulated noise produced, V1 and V2, 
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Maem oypassed and the track values corrupted with noise, ZMAT, 
are retrieved from data file "NOITRAK.STORAG". If NOITRAK=1, 


1 


then the random number generator is “reseeded" fer each 
Peegram run, producing a different set of noise values 
Pater ine ihn a unique noise corrupted track for each run. 
Swe oltatistics Window 
When WINDOW is set to 0, the filter state statistics 
are COmputed after each Monte Carlo run. The error mean, 
variance and position covariance are computed. If WINDOW 
Momoeorl tO an integer, I, such that max k>I>0, the statistics 
Will be computed based on the data compiled during the last 
I iterations of the simulation. If, for example, WINDOW=10, 
the computation of statistics will be based on the data 
obtained during the last ten iterations of k, and all 
previous data is disregarded. 
d. Error Normalization 
By setting NORM = 1, the error (ERR), which is 
defined as the difference between the true track value (TRAK) 
and the estimate (XHAT), 1s normalized. For all other values 


of NORM the relative error is used in computing the 


Beets tics. 
e. Reinitializing Gains 


The program has the option of reinitializing the gains 
to G(0). This can be done by setting HIGH to an integer I, 


such that max k>I>0. If the surface area of the ellipse 


hs 





increases I consecutive times, indicating filter divergence, 
Memecins are reinitialized to G(0). If reinitializing 
gains is not desired as an option, then HIGH is set to some 


arbitrary large number greater than max k. 


ome | KF 

The EXTKF program computes the optimal estimate, XHAT, 
and the optimal one-step prediction, XHK1K, for the 4-state 
nonlinear tracking problem. The EXTKF program has the 
options: gating scheme, track noise, statistics window, 
and error normalization as described for the LINEST program. 
EXTKF has the following additional options: 

a. Reinitializing the Filter 

The program has the option of reinitializing the 

covariance of one-step prediction error matrix, PK1K, by 
Sowers it to ie ee where n is zero or some small integer. 
Mepen 15 Set to an integer I, such that max k>I>0, and the 
surface area of the ellipse increases I consecutive times, 
then the PK1K matrix will be reset to 10°xP.. 

b. Adaptive Q 

EXTKF has the option of automatically increasing or 

decreasing the state excitation matrix, Q, under certain 
conditions by changing the value of the covariance of 
excitation noise, COVW. INCREASE and DECREASE are set to 
maeepers Lf and J respectively, such that max k>I,J>0. If 


the surface area of the error ellipse increases I 


96 





Lt | 


consecutive times, COVW is doubled or increased by some 
Factor. Conversely, if the surface area of the error elliovse 
decreases J consecutive times, COVW is halved or decreased 

by some factor. If COVW is changed, increased for example, 
the value of COVW can be changed again later in the run, 

if the criteria for either increasing or decreasing is met. 
Bmmeteeacdaptive Q is not desired then INCREASE and DECREASE 


eemeoet £O some large number greater than k. 
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PROGRAM LISTING 
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